package com.zhdl.modules.puncturecable.web;

import com.project.common.util.DelayUtil;
import com.zhdl.common.ytools.AsciiToHexTools;
import com.zhdl.hardware.plc.siemens.repository.impl.SiemensPortCmdHandleDaoImpl;
import com.zhdl.hardware.robot.fair.repository.impl.FairPortCmdHandleDaoImpl;
import com.zhdl.modules.puncturecable.service.PunctureCableService;
import com.zhdl.modules.tools.CommonCallback;
import com.zhdl.modules.wallpulley.web.resp.TensileDistanceSseResp;
import lombok.extern.slf4j.Slf4j;
import org.springframework.scheduling.concurrent.ThreadPoolTaskScheduler;
import org.springframework.stereotype.Component;

import javax.annotation.Resource;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.ScheduledFuture;

import static java.lang.Thread.sleep;

/**
 * 穿电片流程
 */
@Slf4j
@Component
public class PunctureCableProcess {
    /**
     * 线程管理
     */
    @Resource
    public ThreadPoolTaskScheduler taskScheduler1;
    Map<String, ScheduledFuture<?>> mapSF = new HashMap<>();

    @Resource
    private SiemensPortCmdHandleDaoImpl siemensPortCmdHandleDao;
    @Resource
    private FairPortCmdHandleDaoImpl fairCmdHandleDao;
    @Resource
    private PunctureCableService punctureCableService;

    /**
     * 上料，下料运动状态
     */
    public boolean isRunningPick = false,isRunningPut = false;
    /**
     * 上料，下料 机器人手中是否有料
     */
    public boolean isPickBolt = false,isPutBolt = false;

    /**
     * 上料，下料机器人 流程
     */
    public int robotPickStep = -1,robotPutStep = -1, delayTime = 4000, plcDelayMillis = 500;
    /**
     * 机器人初始化状态
     */
    public boolean robotInitStatus = false;

    /**
     * 机器人当前位置,plc读出数据
     */
    public String
            robotPickLocation = "",
            robotPutLocation = "",
            plcValue = "",
            plcValue1 = "";

    public String[] robotPickPoint,robotPutPoint;
    /**
     * 机器人-上料 下料网口
     */
//    public String robotPickPort = "192.168.58.2:8080", robotPutPort = "192.168.59.2:8080";
    public String robotPickPort = "192.168.0.101:5033", robotPutPort = "192.168.0.101:5044";


    public void test(){
//        punctureCableService.test(new CommonCallback<String>() {
//            @Override
//            public void statusCallback(String s) {
//                log.info("一直读" + s);
//            }
//        });
    }

    /**
     * 流程-初始化
     */
    public void init() {

        /* Z默认值 */

        log.info("==============> 开始初始化 <===========");
        /*
            1. 初始化网口，PLC && 法奥机器人
         */
//        fairCmdHandleDao.addPort("192.168.58.2:8080");
//        siemensPortCmdHandleDao.addPort("192.168.0.1:502");
//        fairCmdHandleDao.addPort("192.168.59.2:8080");
        fairCmdHandleDao.addPort("192.168.0.101:5033");
        siemensPortCmdHandleDao.addPort("192.168.0.101:502");
        fairCmdHandleDao.addPort("192.168.0.101:5044");

        /* 初始化下料机器人指令 */
        punctureCableService.init();

        /*
            1. 读取机器人当前位置
            2. 判断机器人是否初始化完成
            3. 如果没有初始化完成，则机器人执行初始化动作
         */
        punctureCableService.robotReadLocation(robotPickPort, s -> {
            robotPickLocation = AsciiToHexTools.hexToAscii(s);
            /* 根据坐标调整 流程调整 */
            if ("w".equals(s.substring(0,1))){
                log.info("机器人");
//                robotInit();
            }
            log.info("当前法奥上料机器人的关节坐标为" + s);
        });

        punctureCableService.robotReadLocation(robotPutPort, s -> {
            robotPutLocation = AsciiToHexTools.hexToAscii(s);
            /* 根据坐标调整 流程调整 */
            if ("w".equals(s.substring(0,1))){
                log.info("机器人");
//                robotInit();
            }
            log.info("当前法奥下料机器人的关节坐标为" + s);
        });

        List<String[]> strings = punctureCableService.setGetLocation();
        robotPickPoint = strings.get(0);
        robotPutPoint = strings.get(1);

        /*
         * 下料初始化动作
         */
        DelayUtil.delayedExecution(1000, new Runnable() {
            @Override
            public void run() {
                punctureCableService.robotSendPutAction(robotPutPort, "涂胶上方", s -> {
                    DelayUtil.delayedExecution(1000, new Runnable() {
                        @Override
                        public void run() {
                            punctureCableService.robotSendPutAction(robotPutPort, "待机位", s12 -> {
                                log.info("下料机器人初始化完成");
                            });
                        }
                    });
                });
            }
        });


        /*
            1. 读取Plc值
            2. 读取允许上下料寄存器的值
            3. 判断是否初次上下料
         */
        punctureCableService.readPlcSignal(s -> {
            plcValue = s;
            log.info("当前plc的值为:" + s);
        });

        taskScheduler1.scheduleWithFixedDelay(() -> {

            punctureCableService.readStopStatus(s -> {
                /* 读取信号-开始 */
                String status = s.substring(11,12);
                if (stopStatus && status.equals("0")){
                    if (isRunningPick && robotPickStep != 0){
                        log.info(stopStatus + "---" + status + "---" + isRunningPick + "---" + robotPickStep);
                        robotPickProcess();
                    }
                    if (isRunningPut && robotPutStep != 0){
                        log.info(stopStatus + "---" + status + "---" + isRunningPut + "---" + robotPutStep);
                        robotPutProcess();
                    }
                }
                stopStatus = status.equals("1");
                log.info("当前寄存器值为: " + s + ",当前运行状态为:" + status);
            });

            punctureCableService.readPlc0072(s1 -> {
                /* 允许机器人上料信号 */
                String signal1 = s1.substring(0,1);
                /* 允许机器人下料信号 */
                String signal2 = s1.substring(13,14);
                log.info("当前plc信号信号为" + s1 + ",上料信号->" + signal1 + ",下料信号->" + signal2);
                log.info("当前机器人上料运动状态:" + isRunningPick + ",当前机器人下料运动状态:" + isRunningPut);

                if (!stopStatus){
                    /* 判断是否第一次上料 */
                    if (isPickBolt) {
                        if ("1".equals(signal1)&& !isRunningPick)  {
                            getPickSignalTimes++;
                            log.info("上料通知读取，次数为->" + getPickSignalTimes );
                            punctureCableService.robotToPlc(0,false);
                            log.info("PLC-不允许环形导轨转动");
                            pickStartProcess();
                        }
                    } else {
                        pickStartProcess();
                    }

                    log.info("上料机器人手里有料没有-|>" + isPickBolt);

                    /* 判断是否第一次上料 */
                    isPutBolt = true;
                    if (isPutBolt) {
                        if ("1".equals(signal2) && !isRunningPut) {
                            getPutSignalTimes++;
                            log.info("下料通知读取，次数为->" + getPutSignalTimes);
                            punctureCableService.robotToPlc(1,false);
                            log.info("PLC-不允许环形导轨转动");
                            putStartProcess();
                        }
                    }
                }
            });
        },100);

        DelayUtil.delayedExecution(2000, () -> {
            /* 允许行架动作 */
            action_HangerStop(false);
            log.info("上料机-不允许运动-取消");
            DelayUtil.delayedExecution(100, () -> {
                punctureCableService.robotToPlc(0,false);
                punctureCableService.robotToPlc(1,false);
            });
        });
    }

    public TensileDistanceSseResp tensileDistanceSseResp = new TensileDistanceSseResp();

    int getPickSignalTimes = 0,getPutSignalTimes = 0;

    boolean stopStatus = true;

    /**
     * 穿电片上料流程开启
     */
    public void pickStartProcess() {
        if (isRunningPick) {
            return;
        }
        isRunningPick = true;
        log.info("==============> 上料机器人-->准备运动 <===========");
        DelayUtil.delayedExecution(1000, () -> {
            /* 判断是否为 初次上料 */
            log.info("==============> 上料机器人-->开始运动 <===========");
            if (isPickBolt){
                robotPickStep = 0;
                robotPickProcess();
            } else {
                robotPickStep = 2;
                robotPickProcess();
            }
        });
    }
    /**
     * 穿电片下料流程开启
     */
    public void putStartProcess() {
        if (isRunningPut) {
            log.info("测试-下料流程正在运行");
            return;
        }
        isRunningPut = true;

        DelayUtil.delayedExecution(1000, () -> {
            /* 判断是否为 初次上料 */
            if (isPutBolt){
                log.info("测试-准备动作");
                robotPutStep = 0;
                robotPutProcess();
            } else {
                log.info("测试-开始动作");
                robotPutStep = 0;
                robotPutProcess();
            }
        });

    }

    public boolean pickDone = false, putDone = false;

    public void pickStartProcess(boolean runDone) {
        if (runDone){
            pickDone = true;
            robotPickStep = 0;
            robotPickTestProcess();
        } else {
            pickDone = false;
        }
    }

    public void putStartProcess(boolean runDone) {
        if (runDone){
            putDone = true;
            robotPutStep = 0;
            robotPutTestProcess();
        } else {
            putDone = false;
        }
    }

    public void robotPickTestProcess(){
        if (!pickDone){
            return;
        }

        robotPickStep++;

        switch (robotPickStep){
            case 1 -> {
//                log.info("到底走了没有啊!!!!!!!!!!!!!!!!!!!!!!!!!");
                punctureCableService.robotSendPickAction(robotPickPort, 5, s -> {

                });
                taskGetRobotResult(robotPickPoint[5],s1 -> {
                    robotPickTestProcess();
                });
            }
            case 2 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 6, s -> {

                });
                taskGetRobotResult(robotPickPoint[6],s1 -> {
                    robotPickTestProcess();
                });
            }
            case 3 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 5, s -> {
                });
                taskGetRobotResult(robotPickPoint[5], s12 -> {
                    robotPickTestProcess();
                });
            }
            case 4 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 2, s -> {

                });
                taskGetRobotResult(robotPickPoint[2], s14 -> {
                    robotPickTestProcess();
                });
            }
            case 5 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 1, s -> {

                });
                taskGetRobotResult(robotPickPoint[1], s14 -> {
                    robotPickTestProcess();
                });
            }
            case 6 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 0, s -> {

                });
                taskGetRobotResult(robotPickPoint[0], s14 -> {
                    robotPickTestProcess();
                });
            }
            case 7 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 3, s -> {

                });
                taskGetRobotResult(robotPickPoint[3], s14 -> {
                    robotPickTestProcess();
                });
            }
            case 8 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 4, s -> {

                });
                taskGetRobotResult(robotPickPoint[4], s14 -> {
                    robotPickTestProcess();
                });
            }
            case 9 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 1, s -> {

                });
                taskGetRobotResult(robotPickPoint[1], s14 -> {
                    robotPickTestProcess();
                });
            }
            case 10 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 2, s -> {

                });
                taskGetRobotResult(robotPickPoint[2], s14 -> {
                    robotPickTestProcess();
                });
            }
            case 11 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 5, s -> {

                });
                taskGetRobotResult(robotPickPoint[5], s14 -> {
                    robotPickTestProcess();
                });
            }
            case 12 -> {
                robotPickStep = 0;
                if (pickDone){
                    robotPickTestProcess();
                }
            }
        }
    }

    /**
     * 穿垫片-机器人取料流程流程
     */
    public void robotPutTestProcess(){
        if (!putDone){
            return;
        }

        robotPutStep++;
        switch (robotPutStep){
            case 1 -> {
                punctureCableService.robotSendPutAction(robotPutPort, "等待螺杆", s1 -> {

                });
                taskGetRobotPutResult(robotPutPoint[1], s2 -> {
                    robotPutTestProcess();
                });
            }
            case 2 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "抓取螺杆", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[2], s12 -> {
                    robotPutTestProcess();
                });
            }

            case 3 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "螺杆上方", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[3], s15 -> {
                    robotPutTestProcess();
                });
            }
            case 4 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "涂胶位", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[4], s15 -> {
                    robotPutTestProcess();
                });
            }
            case 5 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "涂胶上方", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[5], s15 -> {
                    robotPutTestProcess();
                });
            }
            case 6 ->{
                boltNum++;
                punctureCableService.robotSendPutAction(robotPutPort, "放料位上" + boltNum, s -> {

                });
                taskGetRobotPutResult(robotPutPoint[4+ (boltNum * 2) ], s15 -> {
                    robotPutTestProcess();
                });
            }
            case 7 -> {
                punctureCableService.robotSendPutAction(robotPutPort, "放料位" + boltNum, s -> {

                });
                taskGetRobotPutResult(robotPutPoint[5+ (boltNum*2) ], s15 -> {
                    robotPutTestProcess();
                });
            }
            case 8 -> {
                punctureCableService.robotSendPutAction(robotPutPort, "放料位上" + boltNum, s -> {

                });
                taskGetRobotPutResult(robotPutPoint[4+ (boltNum*2) ], s15 -> {
                    robotPutTestProcess();
                });
            }

            case 9 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "待机位", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[0], s15 -> {
                    robotPutTestProcess();
                });
            }
            case 10 ->{
                robotPutStep = 0;
                if (putDone){
                    robotPutTestProcess();
                }
            }

        }
    }

    public void pickRelease(){
        isRunningPick = false;
        robotPickStep = -1;
        tensileDistanceSseResp = new TensileDistanceSseResp();
    }

    public void putRelease(){
        isRunningPut = false;
        robotPutStep = -1;
        tensileDistanceSseResp = new TensileDistanceSseResp();
    }

    public void robotPickProcess(){
        if (stopStatus){
            return;
        }
        robotPickStep++;

        switch (robotPickStep){
            case 1 -> {
//                log.info("到底走了没有啊!!!!!!!!!!!!!!!!!!!!!!!!!");
                punctureCableService.robotSendPickAction(robotPickPort, 5, s -> {

                });
                taskGetRobotResult(robotPickPoint[5],s1 -> {
                    if (s1.equals("ok")){
                        log.info("法奥机器人-到达放料点正上方位置");
                        /* 环形导轨-气缸夹紧 */
                        action_closeCylinder(0);
                        taskGetPlcResult(3, s2 -> {
                            log.info("PLC-定位销合上");
                            if ("ok".equals(s2)){
                                robotPickProcess();
                                log.info("法奥机器人-步数:" + robotPickStep + "完成");
                            }
                        });
                    }
                });
            }
            case 2 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 6, s -> {

                });
                taskGetRobotResult(robotPickPoint[6],s1 -> {
                    if (s1.equals("ok")){
                        log.info("法奥机器人-到达放料点位置");
                        /* 环形导轨-气缸松开 */
                        action_openCylinder(0);
                        taskGetPlcResult(4, s2 -> {
                            if ("ok".equals(s2)){
                                log.info("PLC-定位销松开");
                                /* 机器人-夹具松开 */
                                action_closeGripper(0);
                                taskGetPlcResult(1, s3 -> {
                                    if ("ok".equals(s3)){
                                        log.info("PLC-机器人夹具松开");
                                        robotPickProcess();
                                        log.info("法奥机器人-步数:" + robotPickStep + "完成");
                                    }
                                });
                            }
                        });
                    }
                });
            }
            case 3 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 5, s -> {
                });
                taskGetRobotResult(robotPickPoint[5], s12 -> {
                    if ("ok".equals(s12)){
                        log.info("法奥机器人-到达放料点上方位置");
                        robotPickProcess();
                        log.info("法奥机器人-步数:" + robotPickStep + "完成");
                    }
                });
            }
            case 4 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 2, s -> {

                });
                taskGetRobotResult(robotPickPoint[2], s14 -> {
                    if ("ok".equals(s14)) {
                        log.info("法奥机器人-到达原点正上方位置-放料动作");
                        /* 判断是否第一次-如果第一次则不写这个结果 */
                        if (isPickBolt){
                            /* 环形导轨-允许转动 */
//                            action_allowTurn(0);
                            punctureCableService.robotToPlc(0,true);
                            log.info("PLC-允许环形导轨转动");
                            DelayUtil.delayedExecution(plcDelayMillis, () -> {
                                robotPickProcess();
                                log.info("法奥机器人-步数:" + robotPickStep + "完成");
                            });
                        } else {
                            /* 打开夹爪 */
                            action_closeGripper(0);
                            log.info("PLC-机器人夹具松开");
                            taskGetPlcResult(1, s13 -> {
                                if ("ok".equals(s13)) {
                                    robotPickProcess();
                                    log.info("法奥机器人-步数:" + robotPickStep + "完成");
                                }
                            });
                        }
                    }
                });
            }
            case 5 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 1, s -> {

                });
                taskGetRobotResult(robotPickPoint[1], s14 -> {
                    if ("ok".equals(s14)) {
                        log.info("法奥机器人-到达原点正上方-上料动作 ");
                        robotPickProcess();
                        log.info("法奥机器人-步数:" + robotPickStep + "完成");
                    }
                });
            }
            case 6 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 0, s -> {

                });
                taskGetRobotResult(robotPickPoint[0], s14 -> {
                    if ("ok".equals(s14)) {
                        log.info("法奥机器人-回原点完成");
                        robotPickProcess();
                        log.info("法奥机器人-步数:" + robotPickStep + "完成");
                    }
                });
            }
            case 7 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 3, s -> {

                });
                taskGetRobotResult(robotPickPoint[3], s14 -> {
                    if ("ok".equals(s14)) {
                        log.info("法奥机器人-到达抓料点");
                        /* 机器人-夹爪夹紧 */
                        action_openGripper(0);
                        taskGetPlcResult(2, s15 -> {
                            if ("ok".equals(s15)) {
                                log.info("PLC-机器人夹爪夹紧");
                                robotPickProcess();
                                log.info("法奥机器人-步数:" + robotPickStep + "完成");
                            }
                        });
                    }
                });
            }
            case 8 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 4, s -> {

                });
                taskGetRobotResult(robotPickPoint[4], s14 -> {
                    if ("ok".equals(s14)) {
                        log.info("法奥机器人-到达安全位置");
                        action_Discharge(false);
                        log.info("上料机-出料");
                        DelayUtil.delayedExecution(plcDelayMillis, () -> {
                            robotPickProcess();
                            log.info("法奥机器人-步数:" + robotPickStep + "完成");
                        });
                    }
                });
            }
            case 9 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 1, s -> {

                });
                taskGetRobotResult(robotPickPoint[1], s14 -> {
                    if ("ok".equals(s14)) {
                        log.info("法奥机器人-到达原点正上方位置");
                        robotPickProcess();
                        log.info("法奥机器人-步数:" + robotPickStep + "完成");
                    }
                });
            }
            case 10 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 2, s -> {

                });
                taskGetRobotResult(robotPickPoint[2], s14 -> {
                    if ("ok".equals(s14)) {
                        log.info("法奥机器人-到达原点正上方位置 变成抓料点位置");
                        action_Discharge(true);
                        log.info("上料机-出料");
                        DelayUtil.delayedExecution(delayTime, () -> {
                            robotPickProcess();
                            log.info("法奥机器人-步数:" + robotPickStep + "完成");
                        });
                    }
                });
            }
            case 11 -> {
                punctureCableService.robotSendPickAction(robotPickPort, 5, s -> {

                });
                taskGetRobotResult(robotPickPoint[5], s14 -> {
                    if ("ok".equals(s14)) {
                        log.info("法奥机器人-到达放料点正上方位置");
                        robotPickProcess();
                        log.info("法奥机器人-步数:" + robotPickStep + "完成");
                    }
                });
            }
            case 12 -> {
                if (!isPickBolt){
                    isPickBolt = true;
                }

                pickRelease();
                log.info("上料-流程结束");
            }
        }
    }

    /**
     * 穿垫片-机器人取料流程流程
     */
    public void robotPutProcess(){
        if (stopStatus){
            return;
        }

        robotPutStep++;
        switch (robotPutStep){
            case 1 -> {
                action_HangerStop(true);
                log.info("上料机-不允许运动");
                DelayUtil.delayedExecution(100, () -> {
                    action_closeGripper(1);
                    taskGetPlcResult(5, s -> {
                        if ("ok".equals(s)) {
                            log.info("PLC-机器人夹爪松开");
                            punctureCableService.robotSendPutAction(robotPutPort, "等待螺杆", s1 -> {

                            });
                        }
                    });
                });
                taskGetRobotPutResult(robotPutPoint[1], s2 -> {
                    if ("ok".equals(s2)){
                        log.info("法奥机器人-下料-等待螺杆位置");
                        robotPutProcess();
                        log.info("法奥机器人-步数:" + robotPutStep + "完成");
                    }
                });
            }
            case 2 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "抓取螺杆", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[2], s12 -> {
                    if ("ok".equals(s12)) {
                        log.info("法奥机器人-下料-到达抓料位");
                        action_openGripper(1);
                        taskGetPlcResult(2, s13 -> {
                            if ("ok".equals(s13)) {
                                log.info("PLC-机器人夹爪夹紧");
                                action_closeCylinder(1);
                                taskGetPlcResult(7, s14 -> {
                                    if ("ok".equals(s14)){
                                        log.info("PLC-定位销合上");
                                        robotPutProcess();
                                        log.info("法奥机器人-步数:" + robotPutStep + "完成");
                                    }
                                });
                            }
                        });
                    }
                });
            }

            case 3 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "螺杆上方", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[3], s15 -> {
                    if ("ok".equals(s15)) {
                        log.info("法奥机器人-下料-到达抓料位上方");
                        action_openCylinder(1);
                        taskGetPlcResult(8, s14 -> {
                            if ("ok".equals(s14)) {
                                log.info("PLC-定位销松开");
                                robotPutProcess();
                                log.info("法奥机器人-步数:" + robotPutStep + "完成");
                            }
                        });
                    }
                });
            }
            case 4 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "涂胶位", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[4], s15 -> {
                    if ("ok".equals(s15)) {
                        log.info("法奥机器人-下料-到达涂胶位");
//                        action_allowTurn(1);
                        punctureCableService.robotToPlc(1,true);
                        log.info("PLC-允许环形导轨转动");
                        DelayUtil.delayedExecution(plcDelayMillis, () -> {
                            robotPutProcess();
                            log.info("法奥机器人-步数:" + robotPutStep + "完成");
                        });
                    }
                });
            }
            case 5 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "涂胶上方", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[5], s15 -> {
                    if ("ok".equals(s15)) {
                        log.info("法奥机器人-下料-到达涂胶上方");
                        robotPutProcess();
                        log.info("法奥机器人-步数:" + robotPutStep + "完成");
                    }
                });
            }
            case 6 ->{
                boltNum++;
                punctureCableService.robotSendPutAction(robotPutPort, "放料位上" + boltNum, s -> {

                });
                taskGetRobotPutResult(robotPutPoint[4+ (boltNum * 2) ], s15 -> {
                    if ("ok".equals(s15)) {
                        log.info("法奥机器人-下料-到达放料位上方");
                        robotPutProcess();
                        log.info("法奥机器人-步数:" + robotPutStep + "完成");
                    }
                });
            }
            case 7 -> {
                punctureCableService.robotSendPutAction(robotPutPort, "放料位" + boltNum, s -> {

                });
                taskGetRobotPutResult(robotPutPoint[5+ (boltNum*2) ], s15 -> {
                    if ("ok".equals(s15)) {
                        log.info("法奥机器人-下料-到达放料方");
                        action_closeGripper(1);
                        taskGetPlcResult(5, s16 -> {
                            if ("ok".equals(s16)){
                                log.info("PLC-机器人夹爪松开");
                                robotPutProcess();
                                log.info("法奥机器人-步数:" + robotPutStep + "完成");
                            }
                        });
                    }
                });
            }
            case 8 -> {
                punctureCableService.robotSendPutAction(robotPutPort, "放料位上" + boltNum, s -> {

                });
                taskGetRobotPutResult(robotPutPoint[4+ (boltNum*2) ], s15 -> {
                    if ("ok".equals(s15)) {
                        log.info("法奥机器人-下料-到达放料位上方");
                        robotPutProcess();
                        log.info("法奥机器人-步数:" + robotPutStep + "完成");
                        if (boltNum == 5){
                            boltNum = 0;
                        }
                    }
                });
            }

            case 9 ->{
                punctureCableService.robotSendPutAction(robotPutPort, "待机位", s -> {

                });
                taskGetRobotPutResult(robotPutPoint[0], s15 -> {
                    if ("ok".equals(s15)) {
                        log.info("法奥机器人-下料-到达等待位");
                        action_HangerStop(false);
                        log.info("上料机-不允许运动");
                        DelayUtil.delayedExecution(plcDelayMillis, () -> {
                            robotPutProcess();
                            log.info("法奥机器人-步数:" + robotPutStep + "完成");
                        });
                    }
                });
            }
            case 10 ->{
                putRelease();
                log.info("下料-流程结束");
            }

        }
    }


    public int boltNum = 0;

    /**
     * 判断机器人是否到达位置
     *
     * @param now 当前位置
     * @param ago 指令位置
     * @return 是否匹配
     */
    public boolean JudgeReach(String now, String ago){
        String[] nowLocationArray = now.split("III");
        String[] agoLocationArray = ago.split("III");
        String nowLocation = nowLocationArray[4];
        String agoLocation = agoLocationArray[4];
        String[] nowLocations = nowLocation.split(",");
        String[] agoLocations = agoLocation.split(",");

        if (nowLocations.length < 6 || agoLocations.length < 6) {
            return false;
        }

        float[] nowLocationFloat = new float[6];
        float[] agoLocationFloat = new float[6];

        for (int i = 0; i < 6; i++) {
            float vn = Float.parseFloat(nowLocations[i]);
            nowLocationFloat[i] = vn;
        }
        for (int i = 6; i < 12; i++) {
            float va = Float.parseFloat(agoLocations[i]);
            agoLocationFloat[i-6] = va;
        }

        for (int i = 0; i < 3; i++) {
            if (agoLocationFloat[i] >= (nowLocationFloat[i] - 5)
                    && agoLocationFloat[i] <= (nowLocationFloat[i] + 5)){
                log.info("机器人预计位置->" + agoLocationFloat[i] + "机器人实际位置->" + nowLocationFloat[i] + "->true");
            } else {
                log.info("机器人预计位置->" + agoLocationFloat[i] + "机器人实际位置->" + nowLocationFloat[i] + "->false");
                return false;
            }
        }
        return true;
    }


    /**
     * 轮询-机器人是否到位
     * @param callback 回调
     */
    public void taskGetRobotResult(String robotPickPoint, CommonCallback<String> callback){
        final int[] time = {0};
        Runnable task = new Runnable() {
            @Override
            public void run() {
                boolean mark = true;
                while (mark){
                    time[0]++;
                    boolean b = JudgeReach(robotPickLocation, robotPickPoint);
                    if (b){
                        mark = false;
                        log.info("法奥机器人-上料已经到位");
                        callback.statusCallback("ok");
                    }
                    if (!b){
//                        if (time[0] >= 1 && time[0] <= 4){
//                            punctureCableService.robotSendPickAction(robotPickPort, robotPickPoint, s -> {
//                                log.info("上料机器人-发送的次数" + time[0]);
//                            });
//                        }
                        if (time[0] == 100){
                            mark = false;
                            log.info("超时报警--->当前上料机器人位置" + robotPickLocation + "---机器人预计位置" + robotPickPoint);
                            tensileDistanceSseResp.setWarnInfo("上料机器人-判断位置 超时报警");
                        }
                        try {
                            sleep(100);
                        } catch (InterruptedException e) {
                            throw new RuntimeException(e);
                        }
                    }
                }
            }
        };
        taskScheduler1.execute(task);
    }

    public void taskGetRobotPutResult(String robotPutPoint, CommonCallback<String> callback){
        final int[] time = {0};
        Runnable task = new Runnable() {
            @Override
            public void run() {
                boolean mark = true;
                while (mark){
                    time[0]++;
                    boolean b = JudgeReach(robotPutLocation, robotPutPoint);
                    if (b){
                        mark = false;
                        log.info("法奥机器人-下料已经到位");
                        callback.statusCallback("ok");
                    }
                    if (!b){
//                        if (time[0] >= 3 && time[0] <= 5){
//                            punctureCableService.robotSendPutActionCmd(robotPutPort, robotPutPoint, s -> {
//                                log.info("下料机器人-发送的次数" + time[0]);
//                            });
//                        }
                        if (time[0] == 100){
                            mark = false;
                            log.info("超时报警--->当前下料机器人位置" + robotPutLocation + "---机器人预计位置" + robotPickPoint);
                            tensileDistanceSseResp.setWarnInfo("下料机器人-判断位置 超时报警");
                        }
                        try {
                            sleep(100);
                        } catch (InterruptedException e) {
                            throw new RuntimeException(e);
                        }
                    }
                }
            }
        };
    }

    /**
     * 轮询-plc是否到位
     * @param callback 回调
     */
    public void taskGetPlcResult(int model,CommonCallback<String> callback){
//        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(() -> {
//            String signal = punctureCableService.readPlcReachSignal(model);
//            /* 测试 */
//            signal = "1";
//            if ("1".equals(signal)) {
//                ScheduledFuture<?> scheduledFuture1 = mapSF.get("IO-到位");
//                scheduledFuture1.cancel(true);
//                mapSF.remove("IO-到位");
//                DelayUtil.delayedExecution(plcDelayMillis, () -> {
//                    callback.statusCallback("ok");
//                });
//            }
//        }, 200);
//        mapSF.put("IO-到位",scheduledFuture);
        DelayUtil.delayedExecution(plcDelayMillis, () -> {
            callback.statusCallback("ok");
        });
    }


    /**
     * 机器人-夹爪夹紧
     */
    public void action_openGripper(int model){
        if (model == 0){
            punctureCableService.robotToMotor(1, true, s -> {
                log.info("上料机器人-夹爪夹紧");
            });
        } else {
            punctureCableService.robotToMotor(4, true, s -> {
                log.info("下料机器人-夹爪夹紧");
            });
        }
    }
    /**
     * 机器人-夹爪松开
     */
    public void action_closeGripper(int model){
        if (model == 0){
            punctureCableService.robotToMotor(1, false, s -> {
                log.info("上料机器人-夹爪松开");
            });
        } else {
            punctureCableService.robotToMotor(4, false, s -> {
                log.info("下料机器人-夹爪松开");
            });
        }
    }
    /**
     * 机器人-气缸松开
     */
    public void action_openCylinder(int model){
        if (model == 0){
            punctureCableService.robotToMotor(2, true, s -> {
                log.info("上料机器人-打开气缸");
            });
        } else {
            punctureCableService.robotToMotor(5, true, s -> {
                log.info("下料机器人-打开气缸");
            });
        }
    }
    /**
     * 机器人-气缸夹紧
     */
    public void action_closeCylinder(int model){
        if (model == 0){
            punctureCableService.robotToMotor(2, false, s -> {
                log.info("上料机器人-关闭气缸");
            });
        } else {
            punctureCableService.robotToMotor(5, false, s -> {
                log.info("下料机器人-关闭气缸");
            });
        }
    }
    /**
     * 机器人-允许转动
     */
    public void action_allowTurn(int model){
        if (model == 0){
            punctureCableService.robotToMotor(3, true, s -> {
                log.info("上料机器人-允许转动");
            });
        } else {
            punctureCableService.robotToMotor(6, true, s -> {
                log.info("下料机器人-允许转动");
            });
        }
    }

    /**
     * 机器人-不允许转动
     */
    public void action_noTurn(int model){
        if (model == 0){
            punctureCableService.robotToMotor(3, false, s -> {
                log.info("上料机器人-不允许转动");
            });
        } else {
            punctureCableService.robotToMotor(6, false, s -> {
                log.info("下料机器人-不允许转动");
            });
        }
    }

    /**
     * 机器人-信号清零
     */
    public void action_writeFalse(int model){
        if (model == 0){
            punctureCableService.robotToMotor(0, false, s -> {
                log.info("上料机器人-读到允许动作信号，清零");
            });
        } else {
            punctureCableService.robotToMotor(7, false, s -> {
                log.info("下料机器人-读到允许动作信号，清零");
            });
        }
    }

    public void action_Discharge(boolean open){
        punctureCableService.robotToPlc(0, open, s -> {
            log.info("上料机器人-上料机排料" + open);
        });
    }
    public void action_HangerStop(boolean open){
        punctureCableService.robotToPlc(1, open, s -> {
            log.info("上料机器人-行架运动" + open);
        });
    }

    /**
     * 机器人测试
     */
    public void tempInit(){
        /* 临时测试 */
//            tempInit();

        //            if (robotInitStatus){
//                punctureCableService.readPlc0072(s1 -> {
//                    /* 允许机器人上料信号 */
//                    String signal1 = s1.substring(0,1);
//                    /* 允许机器人下料信号 */
//                    String signal2 = s1.substring(13,14);
//                    log.info("当前plc信号信号为" + s1 + ",上料信号->" + signal1 + ",下料信号->" + signal2);
//                    if ("1".equals(signal1) && !isRunningPick) {
//                        /* 信号-读到置零 */
//                        action_writeFalse(0);
//                        pickStartProcess();
//                        log.info("初始化完成，等待通知上料");
//                    }
//                    if ("1".equals(signal2) && !isRunningPut) {
//                        /* 信号-读到置零 */
//                        action_writeFalse(1);
//                        putStartProcess();
//                        log.info("初始化完成，等待通知上料");
//                    }
//                });
//            } else {
//                /* 判断机器人手中是否有料 */
//                log.info("机器人-开始初始化");
//            }


    }

}

